


#include "FlashM.h"

// 0x0019 to 0xFFF7
// Erase 512 rows program memory
void FlashM_Erase(UINT32 startAddr)
{
    TBLPTR = startAddr;
    EECON1bits.WREN = 1;
    EECON1bits.FREE = 1;
    INTCONbits.GIE = 0;

    EECON2 = 0x55;
    EECON2 = 0xAA;

    EECON1bits.WR = 1;

    asm("NOP");
    asm("NOP");
    INTCONbits.GIE = 1;   
}

void FlashM_Read16(UINT32 startAddr, UINT8 *dataHi, UINT8 *dataLo)
{
    TBLPTR = startAddr;
    asm("TBLRD*+");
    *dataLo = TABLAT;

    asm("TBLRD*+");
    *dataHi = TABLAT;
}

void FlashM_Write16(const UINT32 startAddr, const UINT16 dataNew) // Must be even addr
{
    TBLPTR = startAddr;
    TABLAT = (UINT8)(dataNew & 0x00FF);
    asm("TBLWT*+");
    TABLAT = (UINT8)(dataNew >> 8);
    asm("TBLWT*+");

    TBLPTR = startAddr;
    EECON1bits.WPROG = 1;
    EECON1bits.WREN = 1;
    INTCONbits.GIE = 0;
    EECON2 = 0x55;
    EECON2 = 0xAA;
    EECON1bits.WR = 1;
    NOP();
    NOP();
    INTCONbits.GIE = 1;
    EECON1bits.WPROG = 0;
    EECON1bits.WREN = 0;


}

BOOL FlashM_Write64(UINT32 startAddr, UINT16 *dataNew)
{
    TUINT16 tempVar[32];
    UINT16 counter;
    UINT16 i = 0;
    //UINT32 startAddrTemp = startAddr;

    //FlashM_Erase(startAddrTemp);

    TBLPTR = startAddr;
    EECON1bits.WPROG = 0;

    for (counter = 0; counter < 32; counter++)
    {
        tempVar[counter].l = *(dataNew + counter);
    }

    

    for (counter = 0; counter < 32; counter++)
    {
        TABLAT = tempVar[counter].s.Lo;
        asm("TBLWT*+");
        //++TBLPTR;
        TABLAT = tempVar[counter].s.Hi;
        asm("TBLWT*+");
        //++TBLPTR;
    }

    TBLPTR--;
    //TBLPTR = startAddr;
    EECON1bits.WREN = 1;
    INTCONbits.GIE = 0;

    EECON2 = 0x55;
    EECON2 = 0xAA;

    EECON1bits.WR = 1;
    NOP();
    NOP();
    INTCONbits.GIE = 1;
    EECON1bits.WREN = 0;
    //TBLPTR += 32;

    return bTRUE;
}

BOOL Flash_HandleWrite16(const UINT32 startAddr, const UINT16 dataNew)
{
    TUINT16 dataRead;
    if (startAddr % 2 == 0)
    {
        FlashM_Read16(startAddr, &(dataRead.s.Hi), &(dataRead.s.Lo));
        if (dataRead != 0xFFFF)
        {
            FlashM_Erase(UINT32 startAddr);
        }

        FlashM_Write16(startAddr, dataNew);

        return bTRUE;
    }

    return bFALSE;
}